/*
 * Software for Interactive Project Pool Game
 * Columbia University. New York, 2008
 * Authors:
 *   Abdulhamid Ghandour
 *   Thomas John
 *   Jaime Peretzman
 *   Bharadwaj Vellore
 * 
 * Desc:
 */
#include <stdio.h>
#include "debug.h"
#include "fixedpoint.h"
#include "gameconfig.h"
#include "types.h"
#include "ball.h"
#include "i2c.h"
#include "lcd.h"

#define NO_CUE_DETECTED (IORD_32DIRECT(VISION_BASE, 0) >> 31)

static void initPockets();
static void initBalls();
static void initCue();
static void play();
extern int calibrate();
struct ball_t balls[NUM_BALLS];
struct ball_t cue;
struct ball_t pockets[6];

int main(){
  volatile int i;
  DP_HI;

  LCD_Init();
  DP("Welcome to Projection Pool\n");

  LCD_Show_Text("4840 Lab 2");

  configureCamera();
  calibrate();
  initCue();
  initPockets();
    
  while(1){
    initBalls();
    drawBalls(balls);
    play();
    for(i=0; i<1000000; i++);
  }

  DP_BYE;
}

void play(){
  int i,j;
  long long time, earliestEventTime, eventTime;
  event_t eventType;
  struct ball_t *collidingBall1, *collidingBall2;
  edge_t tableEdge, collisionTableEdge;

  long long xin = 0, prevXin = 0;
  long long yin = 0, prevYin = 0;
  long long xcam_resol = 320;
  long long ycam_resol = 480;
  long long A = FPDIV(INT2FP(18),INT2FP(10));
  long long b_margin=15;
  int numBallsPocketed = 0;
  
  while(1){
    time = INT2FP(1LL);
    if(!NO_CUE_DETECTED){
      xin = (long long)IORD_32DIRECT(VISION_BASE, 0);
      yin = ((xin >> 16) & 0x7FFF);
      xin = (xin & 0xFFFF);
      xin = INT2FP(((tableEndX - tableStartX+2*b_margin) * (xin)) / xcam_resol + tableStartX-b_margin);
      yin = INT2FP(((tableEndY - tableStartY+2*b_margin) * (yin)) / ycam_resol + tableStartY-b_margin);
      cue.ballState = BALL_VISIBLE;
      
      if ((prevXin != 0) && (prevYin != 0)){
        cue.pos.x = prevXin;
        cue.pos.y = prevYin;
        IOWR_POS(VGA_BASE, 13, FP2INT(prevXin));
        IOWR_POS(VGA_BASE, 14, FP2INT(prevYin));
        cue.vel.x = FPMUL(A,(xin - prevXin));
        cue.vel.y = FPMUL(A,(yin - prevYin));
        prevXin = xin;
        prevYin = yin;
      }
      else{
          prevXin = xin;
          prevYin = yin;
      }
    }else{
      cue.ballState = BALL_INVISIBLE;
      prevXin = 0;
      prevYin = 0;  
    }

    do{
      earliestEventTime = time;
      eventType = NO_COLLISION;
      collidingBall1 = NULL;
      collidingBall2 = NULL;
      tableEdge = NONE;

      for(i=0; i<NUM_BALLS; i++){
        if(BALL_INVISIBLE == balls[i].ballState){
          continue;
        }

        /*
        // Check for collisions with the table boundaries
        */
        if(isBallMoving(&balls[i]) == TRUE){
          eventTime = collisionWithTableTime(&balls[i],&tableEdge);
          if((eventTime >= 0) && (eventTime < earliestEventTime)){
            earliestEventTime = eventTime;
            collidingBall1 = &balls[i];
            collidingBall2 = NULL;
            eventType = TABLE_COLLISION;
            collisionTableEdge = tableEdge;
          }
        }

        /*
        // Check for "collision" with cue
        */
        if(BALL_INVISIBLE != cue.ballState){
          eventTime = collisionWithBallTime(&balls[i],&cue);
          if((eventTime >= 0) && (eventTime < earliestEventTime)){
            earliestEventTime = eventTime;
            collidingBall1 = &balls[i];
            collidingBall2 = &cue;
            eventType = CUE_COLLISION;
          }
        }

        /*
        // Check for "collision" with pockets
        */
        if(isBallMoving(&balls[i]) == TRUE){
          for(j=0; j<6; j++){      
            eventTime = collisionWithBallTime(&balls[i], &pockets[j]);
            if((eventTime >= 0) && (eventTime < earliestEventTime)){
              earliestEventTime = eventTime;
              collidingBall1 = &balls[i];
              collidingBall2 = &pockets[j];
              eventType = POCKET_COLLISION;
            }
          }
        }

        /*
        // Collision with other balls
        */
        if(isBallMoving(&balls[i]) == TRUE){
          for(j=0; j<NUM_BALLS; j++){
            if(BALL_INVISIBLE == balls[j].ballState){
              continue;
            }

            eventTime = collisionWithBallTime(&balls[i],&balls[j]);
            if((eventTime >= 0) && (eventTime < earliestEventTime)){
              earliestEventTime = eventTime;
              collidingBall1 = &balls[i];
              collidingBall2 = &balls[j];
              eventType = BALL_COLLISION;
            }
          }
        }
      }

      moveBalls(balls, earliestEventTime);

      switch(eventType){
        case NO_COLLISION:
          //DP("NO_COLLISION\n");
        break;

        case POCKET_COLLISION:
          collidingBall1->ballState = BALL_INVISIBLE;
          numBallsPocketed++;
          PLAY_SOUND;
        break;

        case CUE_COLLISION:
          //DP("CUE_COLLISION\n");
          handleCollisionWithCue(collidingBall1,&cue);
          PLAY_SOUND;
        break;

        case TABLE_COLLISION:
          //DP("TABLE_COLLISION\n");
          switch(collisionTableEdge){
            case LEFT:
            case RIGHT:
              collidingBall1->vel.x *= -1LL;
            break;

            case TOP:
            case BOTTOM:
              collidingBall1->vel.y *= -1LL;
            break;

            default:
              DP_ASSERT(0,"Collision with non-existent table edge!");
            break;
          }
          PLAY_SOUND;        
        break;

        case BALL_COLLISION:
          handleBallCollision(collidingBall1, collidingBall2);
          PLAY_SOUND;
        break;

        default:
          DP_ASSERT(0,"Invalid event");
        break;
      };
      drawBalls(balls);
      time -= earliestEventTime;
    }while(time > 0);
    drawBalls(balls);
    applyFriction(balls);

    if(numBallsPocketed == NUM_BALLS){
      return;
    }
  }
}

static void initPockets(){
  int i;

  for(i=0; i<6; i++){
    pockets[i].vel.x = 0;
    pockets[i].vel.y = 0;
    pockets[i].radius = POCKET_RADIUS;
    pockets[i].colour = 1;
    pockets[i].ballState = BALL_INVISIBLE;
  }
  DP_HI;
  printFP(TABLE_WIDTH);
  printFP(TABLE_HEIGHT);
  printFP(FPDIV(TABLE_WIDTH,INT2FP(2LL)));
  pockets[0].pos.x = TOP_LEFT_POCKET_X;
  pockets[0].pos.y = TOP_LEFT_POCKET_Y; 
  pockets[1].pos.x = TOP_MID_POCKET_X;
  pockets[1].pos.y = TOP_MID_POCKET_Y; 
  pockets[2].pos.x = TOP_RIGHT_POCKET_X;
  pockets[2].pos.y = TOP_RIGHT_POCKET_Y; 
  pockets[3].pos.x = BOTTOM_LEFT_POCKET_X;
  pockets[3].pos.y = BOTTOM_LEFT_POCKET_Y; 
  pockets[4].pos.x = BOTTOM_MID_POCKET_X;
  pockets[4].pos.y = BOTTOM_MID_POCKET_Y; 
  pockets[5].pos.x = BOTTOM_RIGHT_POCKET_X;
  pockets[5].pos.y = BOTTOM_RIGHT_POCKET_Y;
  
  DP("Pocket co-ordinates:\n");
  printFP(pockets[0].pos.x);
  printFP(pockets[0].pos.y);
  printFP(pockets[1].pos.x);
  printFP(pockets[1].pos.y);
  printFP(pockets[2].pos.x);
  printFP(pockets[2].pos.y);
  printFP(pockets[3].pos.x);
  printFP(pockets[3].pos.y);
  printFP(pockets[4].pos.x);
  printFP(pockets[4].pos.y);
  printFP(pockets[5].pos.x);
  printFP(pockets[5].pos.y);
  DP("End pocket co-ordinates.\n");
}

static void initBalls(){
  int i;

  for(i=0; i<NUM_BALLS; i++){
    balls[i].radius = BALL_RADIUS;
    balls[i].ballState = BALL_VISIBLE;
  }
  balls[0].colour = COL_YELLOW;
  balls[1].colour = COL_CYAN;
  balls[2].colour = COL_WHITE;
  balls[3].colour = COL_YELLOW;
  balls[4].colour = COL_K1;
  balls[5].colour = COL_K2;
  balls[6].colour = COL_K3;
  
  balls[0].pos.x = INT2FP(tableStartX + 40);
  balls[0].pos.y = INT2FP(tableStartY + 40);
  balls[0].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[0].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));

  balls[1].pos.x = INT2FP(tableStartX + 80);
  balls[1].pos.y = INT2FP(tableStartY + 80);
  balls[1].vel.x = FPDIV(INT2FP(0LL),INT2FP(2LL));
  balls[1].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));

  balls[2].pos.x = INT2FP(tableStartX + 160);
  balls[2].pos.y = INT2FP(tableStartY + 120);
  balls[2].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[2].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));

  balls[3].pos.x = INT2FP(tableStartX + 200);
  balls[3].pos.y = INT2FP(tableStartY + 160);
  balls[3].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[3].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));

  balls[4].pos.x = INT2FP(tableStartX + 200);
  balls[4].pos.y = INT2FP(tableStartY + 100);
  balls[4].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[4].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));

  balls[5].pos.x = INT2FP(tableStartX + 160);
  balls[5].pos.y = INT2FP(tableStartY + 40);
  balls[5].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[5].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));
  
  balls[6].pos.x = INT2FP(tableStartX + 180);
  balls[6].pos.y = INT2FP(tableStartY + 180);
  balls[6].vel.x = FPDIV(INT2FP(0LL),INT2FP(1LL));
  balls[6].vel.y = FPDIV(INT2FP(0LL),INT2FP(1LL));
}

static void initCue(){
  cue.colour = 0LL;
  cue.pos.x = INT2FP(10LL);
  cue.pos.y = INT2FP(10LL);
  cue.radius = INT2FP(2LL);
  cue.vel.x = 0LL;
  cue.vel.y = 0LL;
}

